using System;
using System.Collections.Generic;
using System.Text;
using MyMath;

namespace Physics
{
    public class ConstrainingPlane:ConstrainingObject
    {
        public readonly MyVector Normal;
        public readonly float D;

        public ConstrainingPlane(MyVector normal, float D)
        {
            this.Normal = normal;
            this.D = D;
        }
        public static ConstrainingPlane From3Points(MyVector p1, MyVector p2, MyVector p3)
        {
            MyVector normal = (p2 - p1) ^ (p3 - p1).Normalize();
            ConstrainingPlane plane = new ConstrainingPlane(normal, -normal * p1);

            return plane;
        }
        public static ConstrainingPlane FromNormal(MyVector p, MyVector normal)
        {
            ConstrainingPlane plane = new ConstrainingPlane(normal, -normal * p);
            return plane;
        }

        #region IConstrainingObject Members

        public override bool IntersectSphere(MyVector position, MyVector velocity, CollisionSphere sphere, out MyVector intersectionPoint, out MyVector intersectionNormal, out MyVector translateBack)
        {
            intersectionPoint = new MyVector();
            intersectionNormal = new MyVector();
            translateBack = new MyVector();
            position += -Normal * sphere.Radius;
            double dist = Normal * position - D;
            if (dist<0)
            {
                intersectionNormal = Normal;
                if (velocity.LengthSq > 0)
                {
                    intersectionPoint = position - velocity * (float)(dist / (velocity*Normal));
                }
                else
                {
                    intersectionPoint = position - (float)dist * Normal;
                }
                translateBack = intersectionPoint - position;

                return true;
            }
            return false;
        }

        public override bool IntersectPoint(MyVector position, MyVector velocity, out MyVector intersectionPoint, out MyVector intersectionNormal,out MyVector translateBack)
        {
            intersectionPoint = new MyVector();
            intersectionNormal = new MyVector();
            translateBack = new MyVector();

            double dist = Normal * position - D;
            if (dist < 0)
            {
                intersectionNormal = Normal;
                if (velocity.LengthSq > 0)
                {
                    float time = (float)dist / (velocity * Normal);
                    intersectionPoint = position - velocity * time;
                    
                    if (time > 0 && false)
                        intersectionPoint = position - velocity * time;
                    else
                        intersectionPoint = position - (float)dist * Normal;
                }
                else
                {
                    intersectionPoint = position - (float)dist * Normal;
                }
                translateBack = intersectionPoint - position;

                return true;
            }
            return false;
        }
        #endregion
    }
}
